# import exifread
import glob
from tqdm import tqdm
import numpy as np
import os
import shutil
import math
import pyproj
import pyexiv2

def get_GPSexif(dir):
    '''
    得到图像自带的gps信息
    :param dir: 图像路径str
    :return: 纬度(°)，经度(°)，海拔(m)
    '''

    def read_values(contents, target):
        d, m, s = contents[target].values
        s = float(s)
        value = d + m / 60.0 + s / 3600.0
        return value

    with open(dir, 'rb') as fp:
        contents = exifread.process_file(fp)
        lat = read_values(contents, 'GPS GPSLatitude')
        if contents['GPS GPSLatitudeRef'].values == 'S':
            lat = -lat
        lon = read_values(contents, 'GPS GPSLongitude')
        if contents['GPS GPSLongitudeRef'].values == 'W':
            lon = -lon
        Altitude = float(contents['GPS GPSAltitude'].values[0])
        return lat, lon, Altitude

def ecef2lla(x,y,z):
    ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
    lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
    lon, lat, alt = pyproj.transform(ecef, lla, x, y, z, radians=False)
    return  lat,lon, alt

def lla2ecef(lat, lon, h):
    ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
    lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
    x,y,z = pyproj.transform(lla,ecef, lon, lat, h, radians=False)
    return  x,y,z

def lla_to_ecef(lat, lon, h):
    a = 6378137.0  # 地球长半轴长度
    # f = 1 / 298.257222101  # 地球扁率
    f= 1/298.257223563
    b = (1 - f) * a  # 地球短半轴长度
    e = math.sqrt(1 - (b / a) ** 2)  # 地球的偏心率
    
    lat = lat/180*math.pi
    lon = lon/180*math.pi
    
    N = a / math.sqrt(1 - (e * math.sin(lat)) ** 2)  # 曲率半径

    x = (N + h) * math.cos(lat) * math.cos(lon)
    y = (N + h) * math.cos(lat) * math.sin(lon)
    z = (N * (1 - e ** 2) + h) * math.sin(lat)

    return x, y, z

def ecef_to_lla(x,y,z):
    # pyproj库函数
    # ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
    # lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
    # lon, lat, alt = pyproj.transform(ecef, lla, point_3d[0], point_3d[1], point_3d[2], radians=False)

    # 公式计算
    WGS84_A = 6378137.0
    # WGS84_E = 1 / 298.257222101
    f= 1/298.257223563
    b = (1 - f) * WGS84_A  # 地球短半轴长度
    e = math.sqrt(1 - (b / WGS84_A) ** 2)  # 地球的偏心率
    WGS84_E = e

    b = (WGS84_A * WGS84_A * (1 - WGS84_E * WGS84_E))  ** 0.5
    ep = ((WGS84_A * WGS84_A - b * b) / (b * b)) ** 0.5
    p = math.hypot(x, y)
    th = math.atan2(WGS84_A * z, b * p)
    lon = math.atan2(y, x)
    lat = math.atan2((z + ep * ep * b * (math.sin(th) **3 )), (p - WGS84_E * WGS84_E * WGS84_A * (math.cos(th) ** 3)))
    N = WGS84_A / ((1 - WGS84_E * WGS84_E * math.sin(lat) * math.sin(lat)) ** 0.5)
    alt = p / math.cos(lat) - N
    lat = lat/math.pi*180
    lon = lon/math.pi*180
    return lat,lon,alt

def ecef_to_local(x,y,z):
    transform = np.array([-0.916833527953752,-0.399269685826357,0.0,0.0,
                      0.227936566197776,-0.523405341190006,0.821030919395695,0.0,
                      -0.327812757240844,0.752748674388667,0.570883726687194,0.0,
                      -2093119.2579733,4806380.2032549,3620751.16340286,1.0])
    vec = np.array([x,y,z,1])
    # vec = np.array([x,y,z,1]).reshape([4,1])
    transform = transform.reshape([4,4]).transpose()
    transform_inv = np.linalg.inv(transform)
    # transform_inv = transform.transpose()
    vec3 = np.matmul(transform_inv,vec)
    return vec3

def local_to_ecef(x,y,z):
    transform = np.array([-0.916833527953752,-0.399269685826357,0.0,0.0,
                      0.227936566197776,-0.523405341190006,0.821030919395695,0.0,
                      -0.327812757240844,0.752748674388667,0.570883726687194,0.0,
                      -2093119.2579733,4806380.2032549,3620751.16340286,1.0])
    # vec = np.array([x,y,z,1]).reshape([4,1])
    vec = np.array([x,y,z,1])
    transform = transform.reshape([4,4]).transpose()
    vec3 = np.matmul(transform,vec)
    return vec3[0],vec3[1],vec3[2]

datadir = "H:/data/zzdxOSDB/raw_images/Images/"
datadir_out = "H:/data/zzdxOSDB/query_images_2/Images/"
out_pos_txt_path = "H:/data/zzdxOSDB/query_images_2/query_images_pos.txt"
out_pos_file = open(out_pos_txt_path,'w+')
out_pos_file.write("#x y z heading pitch roll lon lat hei\n")
files = glob.glob(datadir + "*.JPG")
pbar = tqdm(total=len(files))

index = 0
for file in files:
    pbar.update()
    
    # index+=1
    # if index != 4:
    #     continue
    
    # index = 0

    xmpinfo = None
    with pyexiv2.Image(filename=file) as img:
        # exif = img.read_exif()
        # comment = img.read_comment()
        # icc = img.read_icc()
        # iptc = img.read_iptc()
        # raw_xmp = img.read_raw_xmp()
        # thumbnail = img.read_thumbnail()
        xmpinfo = img.read_xmp()
    if xmpinfo is None:
        continue
    lon = float(xmpinfo['Xmp.drone-dji.GpsLongitude'])
    lat = float(xmpinfo['Xmp.drone-dji.GpsLatitude'])
    hei = float(xmpinfo['Xmp.drone-dji.AbsoluteAltitude'])
    yaw = float(xmpinfo['Xmp.drone-dji.GimbalYawDegree'])
    pitch = float(xmpinfo['Xmp.drone-dji.GimbalPitchDegree'])
    roll = float(xmpinfo['Xmp.drone-dji.GimbalRollDegree'])
    
    if roll > 10 or roll < -10:
        continue

    #Xmp.drone-dji.GimbalYawDegree
    #Xmp.drone-dji.GimbalPitchDegree
    #Xmp.drone-dji.GimbalRollDegree
    #AbsoluteAltitude
    #GpsLongitude
    #GpsLatitude
    #RelativeAltitude
    
    # lat, lon, Altitude = get_GPSexif(file)
    x,y,z = lla_to_ecef(lat,lon,hei)
    # l1,l2,h2 = ecef2lla(x,y,z)
    arr = ecef_to_local(x,y,z)
    arr = arr[0:-1]
    
    x1,y1,z1  = local_to_ecef(arr[0],arr[1],arr[2])
    lat1,lon1,h1 = ecef_to_lla(x1,y1,z1)
    
    str2 = '@'.join([str(f) for f in arr.tolist()])
    str2 += "@" + str(yaw)
    str2 += "@" + str(pitch)
    str2 += "@" + str(roll)
    str2 += "@" + str(lon)
    str2 += "@" + str(lat)
    str2 += "@" + str(hei)
    
    out_pos_file.write(str2+"\n")
    str2 = '@' + str2 + '@.jpg'
    newf = datadir_out + str2
    cmd = "copy "+ file + " " + newf
    shutil.copy(file,newf)
    
pbar.close()
out_pos_file.close()